package org.klarton.filter.goertzel;

/**
 * This is the implementation of the Goertzel-algorithm.
 * See also http://en.wikipedia.org/wiki/Goertzel_algorithm
 */
public class GoertzelAlgorithm {

    private final double coeffizient;
    private double s_prev = 0;
    private double s_prev2 = 0;

    public GoertzelAlgorithm(double targetFrequency, double sampleRate) {
        final double normalizedFrequency = targetFrequency / sampleRate;
        coeffizient = 2 * Math.cos(2 * Math.PI * normalizedFrequency);
    }

    public void read(double sample) {
        double s = sample + coeffizient * s_prev - s_prev2;
        s_prev2 = s_prev;
        s_prev = s;
    }

    public double getValue() {
        return s_prev2 * s_prev2 + s_prev * s_prev - coeffizient * s_prev*s_prev2;
    }

    public void reset() {
        s_prev = 0;
        s_prev2 = 0;
    }
}